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ABSTRACT 

It has been claimed in the archival literature that the 
covariance matrix of a Kalman filter, which is designed 
to estimate the quaternion-of-rotation, is necessarily rank 
deficient because the normality constraint of the 
quaternion produces dependence between the quaternion 
elements. In reality, though, this phenomenon does not 
occur. The covariance matrix is not singular, and the 
filter is well behaved. Several simple examples are 
presented that demonstrate the regularity of the 
covariance matrix. First, a Kalman filter is designed to 
estimate variables subject to a functional relationship. 
Then the particular problem of quaternion estimation is 
analyzed. It is shown that the discrepancy stems from the 
fact that the functional relationship exists between the 
elements of the quaternion but not between its estimated 
elements. 

I. INTRODUCTION 

The quaternion-of-rotation is a four-element 
parameterization of attitude, and since the quaternion is 
normal, one element is redundant. This fact has brought 
researchers [see e.g. Ref. 1] to the conclusion that when a 
Kalman filter (KF) is used to estimate all four parameters 
of the quaternion-of-rotation, the filter covariance matrix 
is necessarily singular. The argument behind this 
assertion is that the dependent variables cause 
singularities. However, as will be shown in the ensuing 
discussion, the assertion that the covariance matrix is 
singular when all four elements of the quaternion are 
estimated is not necessarily true, and if it happens to be 
singular, it is not because of the quaternion normality. 

We note that there are two principal approaches to 
the application of an extended KF (EKF) to quaternion- 
of-rotation estimation; namely, the multiplicative 
approach that yields the multiplicative EKF (MEKF) and 
the additive approach that yields the additive EKF 
(AEKF). In the MEKF the difference between the 
estimated and the true quaternion is defined as a 
quaternion-of-rotation between the true coordinate 
system, and the estimated coordinate system. In the 
estimation process the components of this difference 
quaternion are estimated, and are then used to update the 


a-priori estimate of the full quaternion. Because the 
difference itself is defined as a quaternion, this update is 
performed through a quaternion multiplication [1, 2] 
-hence the name multiplicative approach . Since this 
difference is a quaternion, its length is unity and, 
therefore, one of its components is a deterministic 
function of the other three. 

In the AEKF [3], the difference betw-een the true and 
estimated quaternions is defined as a simple subtraction 
of one vector from the other. Using this approach the 
difference is estimated and then the estimate is added to 
the a-priori full quaternion estimate hence the name 
additive approach. This vector difference does not 
constitute a quaternion; therefore, its length is not 
necessarily equal to unity. In fact, if the quaternion and 
its estimate are close to one another, then surely the 
difference quaternion is not of unit length. Naturally, the 
unity constraint is not imposed on the elements of the 
difference quaternion, which become part of the 
estimated state vector. Therefore, the corresponding 
covariance matrix is not inherently singular. 

In this paper we show three realities. First we show 
that even if there is a functional relationship between the 
true values of states, this relationship does not 
necessarily exist between their estimates ; therefore, the 
filter-computed covariance matrix is not necessarily 
singular. Second we show that even if such a relationship 
is imposed on the estimates, the covariance is still not 
necessarily singular, and, third, we show that these 
claims hold, in particular, for the case of quaternion 
estimation when the additive approach is employed. 

We establish these realities in an evolutionary 
manner. We start our presentation with a conceptual 
example of estimating the vertices of a rotating square. 
This example illustrates the first reality; namely, even if 
there is a functional relationship between the true values 
of states there is not necessarily a relationship between 
their estimates. Then we present a simple linear example 
of estimating the position of the edges of a sliding rod. 
Here we demonstrate numerically the same reality and 
also the second reality; namely, even if a relationship is 
imposed on the estimates, the covariance is still not 
necessarily singular. To demonstrate that these realities 
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exist when the quaternion is estimated, we start with a 
simple linear example of estimating the four elements of 
a quaternion when the measurements are quaternion 
measurements. We choose a static case where the body is 
not rotating. This choice enables us to analytically prove 
the first reality for this case. In the final example we treat 
the classical case where the body rotates and the 
measurements are vector measurements. For this case we 
numerically demonstrate that both realities exist when 
quaternions are estimated using the AEKF; namely, the 
filter-computed covariance matrix is non-singular, and it 
remains non-singular even when normality is imposed on 
the filter estimates in a brut force manner. To explain the 
results of the latter example we analyze the operation of 
the ordinary EKF, which is actually an AEKF, and show 
that the forced normalization of the estimated quaternion 
has no bearing on the covariance matrix. 

II. ESTIMATION OF SQUARE VERTICES 
We start our presentation with a conceptual example 
of estimating the vertices of a rotating square. This 
example illustrates the fact that even if there is a 
functional relationship between the correct values of 
estimated states there is not necessarily a relationship 
between their estimates. Consider the system described 
in Fig. 1 where a square is placed on a disk that turns at 
an angular velocity, a> . We obtain noisy measurements 
of the vertices of the square and try to estimate the 
location of these vertices on the disk. Suppose that our 
initial estimate places the vertices at x,, x 2 , x 3 and x 4 . 


CO 


locations of the vertices of a square, knowing the 
location of three of them, say a 1? a 2 anda 3 , we can find 
a 4 , the fourth of them. This, however, does not mean 
that if we know' y^y 2 and y 3 we also know y 4 . As is 
indeed shown in Fig. 1, y|,y 2} y 3 ,y 4 do not form a 
square. In other words, the fact that we know that there is 
a deterministic relationship between the four vertices of 
the square does not mean there is also a deterministic 
relationship between their estimates. Similarly, the fact 
that there is a deterministic relationship between the four 
elements q i , q 2 > Q 3 an d Qa °f a quaternion does not 
mean there is also a deterministic relationship between 
their corresponding estimates q i , q 2 > ^3 am * 3 4 • 

Moreover, if we use the normality constraint to compute 
one element of the quaternion as a function of the other 
three estimated elements, the result wall not necessarily 
be equal to the estimate of that element. 

III. ESTIMATION OF THE EDGES OF A ROD 

After having seen conceptually that there is no reason 
to assume that an algebraic relationship that exists 
between the states of a system is also carried to their 
estimates, we move forward to numerically demonstrate 
this fact and the assertion that even if the relationship is 
imposed on the estimates, the filter covariance is still not 
necessarily singular. 

Consider the rod shown in Fig. 2. It slides along the 
x-axis at a constant velocity V. The coordinates of its 
edges are Sj and s 2 , respectively. In order to describe 
the equations of motion of the two edges in the state 
space we define the following state variables 



After the second measurement update, the estimates 
move to points > f j > y 2 » Y 3 anc ^ >'4 respectively and so 
on. The estimates keep moving along the curved 
trajectories until they reach an infinitesimal distance 
from the vertices a 1 ,a 2! a 3 and a 4 . (Fig. 1 show's the 
position of the square at the end of the estimation 
process,) Because there is a relationship between the 


Fig. 2: Moving rod 


In matrix form these equations are 
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and in a discrete form they are 
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0 
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1 
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X 3 


0 

0 
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At 

X 3 
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0 

0 

0 
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where At is the time difference between the discrete 
time points t k and t k+] . We assume that at the time 
point designated as zero, the rear end of the rod, Sj , 
passed by the origin, 0, at the constant velocity, V. 
Therefore the true initial state vector is 


0 

V 

L 

V 


(3) 


The measured quantities are the positions of Sj and s 2 
on the x-axis. It is assumed that these measurements are 
contaminated by zero mean white noise signals v t and 
v 2 respectively, thus the measurement vector is 


z 


m 


X, + v ] 

X 3 + v 2 


(4) 


Using the following data 

L = 2 m ; V = 1 m/sec ; At = 1 sec ; c V( = ct Vj = 0.1 m 

(5) 

Eqs. (1) through (4) are used to simulate the correct state 
vector and the measurements. A linear Kalman filter 
provides estimates of the state vector. To avoid the well- 
known divergence phenomenon that occurs in unexcited 
state vector dynamics [4], zero mean white process noise 
is added to the dynamics equation in the filter only . A 
covariance matrix, is added to the time-propagated 
filter covariance matrix. The matrix is a diagonal matrix 
with the values 

Q k =diag|lCr ] 10 ” 1 10 -] 1 j ( 6 ) 

Our estimate of the initial state is 


where 6 j is 0 . 2 , 5 2 is 0.1, 5 3 is -0.2, and 5 4 is -0.1. 
Accordingly, we set the initial covariance matrix to be 
the following diagonal matrix 

P 0 = diag{(3 0.2 ) 2 (30. 1) 2 (3 0.2 ) 2 (30.1) 2 j(8) 

The filter is run for 20 sec and in-spite of the following 
dependence between the states Xj and x 2 

x 3 = Xj+L (9) 

no singularity is observed in the filter covariance matrix . 
This is evident by a simple inspection of Fig. 3 where the 
behavior of the filter covariance matrix eigenvalues is 
presented. 



W 

Fig. 3: Eigenvalues of the covariance matrix 


It is not surprising that no singularity occurs in the 
filter covariance matrix because the relationship between 
x 3 and xj is not imposed in the filter model. One may 
speculate that imposing the relationship on the estimates 
x 3 and Xj will cause the matrix to become singular. In 
order to examine this proposition, we imposed the 
distance constraint between the rod end-points by forcing 
the new a-posteriori estimates of Xj and x 3 to be 


X| (+) -y[X](+)+ X,(+)-L] 

(10. a) 

X3(+) = -j[X|(+)+ X 3 (+)+ L] 

(lO.b) 
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(Note that as a result of this change, an additional error 
term must be accounted for, since the state estimates are 
altered. This term is included in the filter propagation 
and update stages according to the discussion presented 
in Sections VII and VIII.) No singularity in the 
covariance can be detected as shown in the eigenvalue 
history in Fig. 4. 


Eigenvalues of tte Esiimaiioft Error Covanarcs MsSrix ( >= >= ) 



Fig. 4: Eigenvalues of the covariance matrix in the 
constrained case. 

IV. QUATERNION ESTIMATION WITH 
QUATERNION MEASUREMENTS 
Next in our evolutionary exposition of the 
singularity issue we demonstrate the lack of covariance 
singularity when the quaternion is estimated. We start 
with a simple linear example of estimating the four 
elements of a quaternion when the measurements are 
quaternion measurements. We choose a static case where 
the body is not rotating. This choice enables us to prove 
analytically that the filter-computed covariance matrix of 
the non-normalized quaternion estimate for this case is 
not singular. 

Consider a rigid satellite body, fixed in inertial space. 
The system equations that describe this simple, static 
case are 

q k = [q k _, (Ha) 

qm,V: = I( lk +v k ( llb l 

where I is the fourth order identity matrix, and q m k is a 
measurement of the quaternion at time, t k . This system is 
linear; therefore, the ordinary KF can be applied in 
estimating q. Consequently a covariance analysis can be 
carried out which is independent of the state or of its 
estimate. Let us assume that 

P 0 = a 2 I (12. a) 


The recurrence relations that describe the covariance 
propagation are 

Pk C~) = A k-1 P k-1 ( ^) A k-1 +Qk-1 ( l3 - a ) 

K k =Pk(-)Hif[H k P k (-)Hl +R k r' 03-b) 
P k (+) = [I-K k H k ]P k (-) (13.c) 

where A k =H k =I. In this example we choose 
Q k _] = 0 . Let us consider a case where R k = rl . Eqs. 
(13) become 


P k (-) = P k -i(+) 

K k = P k (-)[Pk(-)+nr' (14 b) 

P k (+) = [l-Kk]P k (") 04 c) 

Using Eq. (14. a), Eqs. (14.b) and (14. c) can be written as 

K k =P k _,(+)[Pk-i(+) + rir' 04 d) 

P k (+) = [I-K k ]Pk-i(+) (14.e) 


Since P 0 is a diagonal matrix, all the matrices in Eqs. 
(14) are diagonal. Therefore, we can write the last 
equation for any of the elements of P k (+) in terms of the 
same element in P k _i(+). Dropping the plus sign, we 
obtain the following recurrence relation for any element 

of P k (+): 


Pk-l +r 

with 

P0=s 2 (15 b) 

It is quite obvious that p k , which is an eigenvalue of 
P k (+) , is not zero and thus the covariance matrix of the 
filter is not singular. 

V. ESTIMATION OF A QUATERNION FROM 
VECTOR MEASUREMENTS 
In this final example we treat the classical case where 
the body rotates and the measurements are vector 
measurements, we numerically demonstrate that the 
filter-computed covariance matrix is non-singular both 
when we do and do not impose normality on the 
estimated quaternion. First we present the simulation, 
followed by the filter development. 

V.l Simulation 

Without loss of generality, we consider a case where 
the body rotates at the constant angular rate. Let the rate 
vector in this simulation be 

g)=[ 0.1 0.2 0.3] T rad/sec (16) 
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The quaternion at time t 


k+i 


as a function of its value at 


Using Eq. (20) we obtain 


M 

r 2 


time t k is given by [5, p. 512] 

inoo-At 

Qk + i =e ^ . (l7) 

We assume that two vector measurements are obtained at 

each time point, t k . They constantly point at two 

celestial objects located 90° apart and are given in the 
reference coordinates as 

= [l o 0] T OM 

,=[oio] T 08 b) 

In body coordinates we simulate two corresponding 
measured vectors as 

b lk+ , =D(q k+ ,)r, +v s CM 

b 2>k+ i = D fak+l) r 2 + v 2 (19b ^ 

where v, and v 2 are uncorrelated, Gaussian white 
sequences with covariance R each and [5, p. 414] 

■ q 2 _ q | -ql 2 (q,q:> +q 3 q< ) 

D(q k ^)= 2(q,q 2 

2 (q i q 3 + q 2*3 4 ) 

( 20 ) 

In add, t, on to b,, k+1 and b 2>k+1 the filter is also 
furnished with r, , r 2 , u and the covariance matrix R. 

V.2 Filter development 
V.2.1 Dynamics 

The filter dynamics equation is simply 


D(q)r = 


(q? -q? -Q 3 +q5)r, + 2(q,q 2 +q3q<)ri +2(^3 
2 (q,qj -q 5 q 4 )r, + WaXi+XW* 

2 (q ] q 3 + q 2^4 + 2 ^2M3 “ M 1^4 ) J 2 + ( ^ 


where r, , r 2 and r 3 are the components of r. Define 

9[D(q)r] 


H 


k+1 


5q 

then using Eq. (23) we obtain 


(23) 

(24) 


qk*i(-) 


2(q jq 3 ~ Q29 4 ) 

2(q 2 <b +^1^4) 

2(q 2 qj -QIQ4) -Q? "<12 +1 l3 + c lf 


-qf + ql -q 3 + 4« 


(q,r, +q 2 r 2 +q 3 r 3 ) Hl2 r i + q , r 2 -q<r 3 ) 

(q 2 q -q,r 2 +q<r 3 ) (qpi + q 2 r 2 + q 3 r 3 ) 

(q 3 n ~q « r 2 - qi r 3 ) (q 4 n+q 3 r 2 -q 2 r j) 

(-q 3 r i+q4 r 2 + 4i r 3 l (q 4 r i + 4 3 r 2 _c f 2 '' 3 ) 

(-q 4 r, -q 3 r 2 + 42 f 3 ) (-q 3 f i + c i4 r 2 + q, r 3 ) 
(q,r, +q 2 r 2 + q 3 r 3 ) (q 2 r i “ qi r 2 + q4 r 3 > 


and Eq. (22.d) becomes 

z k+l =H k+ idqk+l +v k+l 


k»«(-l 

(25) 


(26) 


in(u)At . 

q k+ . = e2 ^ 


( 21 ) 


V.2.2 Measurements 

The effective measurement equation for the AbRf is 
developed as follows 

z w+ i =b k+1 -b k+1 =D(q k+1 )r-D(q k+1 (-))r + Vk + i 

(iz.a) 


The filter is propagated and also updated every 
second. At every update point we have two effective 
vector measurements, z lk+) and z 2 k+ i . The algorithm 
is a standard EKF algorithm. We ran two cases, one 
without normalization of the estimated quaternion and 
one with normalization. Fig. 5 shows the attitude error 
when normalization is not imposed on the estimated 
quaternion, with an initial error of 10 degrees^ Fig. 6 
shows the covariance matrix eigenvalues, with the scale 
reduced to show the steady-state behavior. Fig. 7 shows 
the deviation from unity of the norm of the estimated 
quaternion. Figs. 8 and 9 present the results for the case 
where normalization is imposed on the estimated 
quaternion. Note that the eigenvalues do not change as a 
result of the quaternion normalization. (See Sections ! 
and VIII for a discussion on the changes to the bK 
algorithm as a result of the normalization.) 


z k+! = D(q k +i (-) + bq k +l) r — D (qk+l( — )) r + V|t+ 1 

5[D(q)r] 


z k+ l =D(q k+) (-))r + - 


3q 


(22. b) 

dq k+l ” D(q k+1 (~)) r + v k+1 


Ql.l 


z k+l 


9 [P(q)r] | 

3q 


dq k +l + v k+l 


(22. c) 
(22 .d) 


lq k +.(-) 
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Fig. 6: Covariance matrix eigenvalues. 



Fig. 7: Norm error of the estimated quaternion 



Fig. 8: Attitude error in the constrained case 



Fig. 9: Covariance matrix eigenvalues. 

VI. THE FULLY RESET ADDITIVE EKF 
To explain the results obtained in the preceding 
examples, without the imposed constraints, we need to 
analyze the operation of the A EKF, and for that we need 
to review the EKF algorithm. We do it in a manner that is 
somewhat different from the usual textbook algorithm 
development, but the resultant algorithm is the standard 
EKF. (We need to adopt this approach for the 
development that will be presented in the following 
section.) Here we treat the special case that is applicable 
to quaternion estimation, where the measurement model 
is nonlinear, but the dynamics model that describes the 
state propagation is linear. 

VI. 1 Measurement Update 

A measurement vector y k , at time t k , is related to 

the state vector X k , at time t k , according to the 
following nonlinear vector function 

y k = h (X k ) + v k (27) 

where V k is a zero mean white noise vector. Suppose 
that at this time point we have an a-priori estimate, 
X k (-), of this state vector. We wish to use the 
measurement y k to improve this a-priori estimate. The 
improved estimate is called the a-posteriori estimate and 
is denoted by X k (+). The KF was developed for linear 
measurements whereas the measurement equation, Eq. 
(27), is nonlinear. However, as will be shown in the 
ensuing discussion, the EKF estimates the difference, e , 

betw>een the true state vector, X, and its estimate f X, 
and the model treated by the EKF is, to first order, a 
linear model in e . 
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We recall that the state update equation 

in the linear 

KF is 


X k (+) = X k (-) + K k [y k -y k ] 

(28. a) 

where 


y k =H k x k (-) 

(28. b) 

which results from the linear model 


Yk = H k X k + v k 

(28. c) 


e k (+) = e k (-) + K k z k (37) 

Substitution of Eq. (36) into Eq. (37) yields 

e k M = e k (-)-K k H k e k (-)-K k v k (38) 

which can be written as 

e k (+) = [I “ K k M k ]e k (-*) ~ K k v k (39) 


In EKF terminology the term y k -y k is known as the 
effective measurement , which we denote by z k ; that is 
*k=y k -y k (29) 

Using Eq. (27) we can write the effective measurement 
as follows. 

z k =h(X k ) + v k -y k (30) 

Note that y k , in Eq. (28. b), was obtained from the linear 
measurement equation (28. c) by dropping the noise 
vector and substituting X k (-) for X k . If we do the 
same in Eq. (27) then Eq. (30) becomes 

z k =h(X k ) + v k -h(X k H) (31) 


Assuming (the knowingly inaccurate assumption) that 
the filter is unbiased; that is, E{e k (+)} = 0 , the updated 
estimation error covariance is computed as follows 

P k (+) = E{e k (+)e k (+)} (40) 

Substituting Eq. (39) along with the fact that 
E{e k (-)v k } = 0 , and the following notations 

R k = E{v k v k } (41) 

P k (— ) = E(e k ( _ )e k (-)} (42) 

Eq. (40) becomes 

P k (+) = [I - K k H k ]P k (-)[I-K k H k ] T + K k R k K k (43) 

X 


As mentioned before, we denote by e the difference 
between the true and estimated state vector; that is, 

e = X - X (32) 

Because X = X + e, we can expand h(X k ) in a Taylor 

series about X , drop the second and higher order terms 
in e, and substitute the result into Eq. (31). This yields 
z k as a linear function of e. The first order expansion of 

h(X k ) about \{~) yields 

h(X k ) = h(X k (-)) + ^^ *k(") O 3 ) 

X=X k (-) 

and substitution of the last equation into Eq. (31) gives 



<?h(X) 

z k = 

k ax 

Let 

H k = 

then Eq. (34) can be written as 

z k = H k e k (-)+ v k (36) 

Following the measurement-update equation of the state 
vector in Eq. (28. a) we write 


X=X k (- 

9h(X) 


ax 


(35) 


Fig. 10: The state, its estimate, the difference and its 
estimate at time t k . 

The sequence of events associated with the 
measurement update in the EKF is presented in Fig. 10 
and can be summarized as follows: 

1. X k-l (+) is propagated from time t k _ 1 to time point 

t k to become X k (-). 

2. The difference X k -X k (-) is e k (-). 
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3. The measurement update yields e k (+), an estimate of 
e k- (") • 

4. e k (+) is added to X k (-) to form X k (+) which is 
then propagated to time point t k+1 to become 
X k+] (-) . We call this operation full reset. Note that 
once e k (+) is absorbed in X k (+) it is not propagated 
separately. 

Once e k (+) is added to X k (-) a full reset is performed, 
and there is no e k (+) to be propagated forward; 
therefore, e k (-), the a priori estimate of e at the next 

time update point, is also zero. 

Using the preceding explanation, the EKF algorithm 
at the measurement update can be summarized as 
follows. 


At a measurement update: 

Signal: 

e k H = 0 

e k (+) = e k (-) + K k (y k - y k ) 
X k (+)=X k H + e k (+) 


(44 .a) 
(44. b) 
(44. c) 


Substituting X k _) given in Eqs. (47) into Eq. (46) yields 
X k (-) + e k (-) = A k _,X k _, (+) + A k _,e k _, (+) + w k _, (58) 

Since the dynamics model of (21) is linear [6, p. 75 ] 

X k (-) = A k _,X k _i(+) (59) 

Subtracting the last equation from Eq. (48) yields 


e k (-) = A k _ 1 e k _ 1 (+) + w k _ 1 (50) 

Using these results we can now examine the time 
propagation of the state estimate and the covariance 
matrix in this particular EKF. Assuming E{e k (-)} = 0 , 
the propagated covariance matrix is defined as 

P k (-)= E{e k (-)e k (-)| (51) 

Substitution of e k (-), given in Eq. (50), into the last 
expression, given that e k _,(+) and w k _, are 
uncorrelated, and using the notation 
q = E{>v k _,w k _,} we obtain the well known result [6, 

p. 76 ) 

P k (-) = A k _]P k _i(+)A k _i +Q k _i (52) 


Note that a sequential substitution of Eqs. (44.a, b, c) 
yields the following textbook expression for the state 
measurement-update equation 

X k (+) = X k (-) + K k (y k - y k ) (44-d) 

Covariance : 

P k (+) = [1 -K k H k ]P k (-)[I-K k H k ] T (45) 
+ K k R k K k 

In the next section we will use the formulation of the state 
measurement-update given in Eqs. (44.a-c) to show the 
effect that normalization has on the state measurement- 
update algorithm. This cannot be shown when the 
textbook expression of Eq. (44. d) is used. 

VI.2 Time Propagation 

Consider the case where the discrete process 
equation that describes the time propagation of a general 
state vector is given by 

x k =A k _ l X k _ 1 +w k _, (46) 

From Eq. (32) and as illustrated in Fig. 10 

X k _i = X k _j(+) + e k _|(+) (47.a) 

and 

X k = X k (-) + e k (-) (47.b) 


Lisina Eqs. (49) and (52) we can summarize the time 
propagation stage of the EKF when the dynamics model 
is linear. 

Time update: 

Signal. 

X k (-) = A k _iX k _] (+) (53) 

Covariance : 

P k (-)=A k -iP k -l(+)A k -l +Qk-1 ( M ) 

We realize from the preceding development that the 
AEKF that is used to estimate the quaternion when no 
normalization takes place is the ordinary EKF algorithm. 
Moreover, it is obvious that the fact that the true 
quaternion is a normal vector has no bearing on the filter 
covariance matrix. This is, of course, also true for all the 
linear examples presented before. 

VII. THE PARTIALLY RESET ADDITIVE 
EXTENDED KALMAN FILTER 

To explain the results obtained in the numerical 
examples, when constraints were imposed, we need to 
consider a special version of the EKF. For this we will 
make use of the developments presented in the previous 
section. 

VII. 1 Time Propagation 

Figure 1 l.a describes the steps of the ordinary (fully 
reset) EKF at the measurement update stage as described 
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in the preceding section and illustrated in Fig. 10. We 
have shown that after computing e k (+), the estimate of 

e k , it is added to X k (-) to form X k (+) which is then 
propagated to time point t k+ , to become X k+1 (-). 
Since e k (+) in its entirety is added to X k (+), this 
operation constitutes a full reset. Because e k (+) is 

propagated forward as a part of X k (+), it is not 
propagated independently. Suppose now that only a part 
of e k (+), denoted in Fig. lib by A k , is added to 

X k (— ) to form X k (+) which is then propagated to time 
point t k+1 to become X k+I (-) . This partial reset \wvzs 
e k (+) out of the propagated full state vector. Therefore 

e k (+) has to be propagated forward separately. Jt is easy 

« * 

to show that when the dynamics model is linear, e k (+) 
is propagated according to 

e k+1 (-) = A k e k (+) (55. a) 

Reducing the index by 1 yields 

e k (-) = A k .,e* k .i(+) <55.b) 


We note in Fig. ll.b that the partial reset, does not 
change the value of the actual estimation error, e k (+), 

therefore the partial reset does not influence the 
covariance matrix because the latter is the covariance 
matrix of e, the estimation error itself. 

The AEKF algorithm for the time propagation stage 
in this partially reset case is then as follows. 


At a time update 

Signal: 


X k (-) = A k _,X k _,(+) 

(56. a) 

e k (-) = A k _ 1 e k _ 1 (+) 

(56 b) 

Covariance : 


P k (-) = A k _,P k _ 1 (+)A k _, +Q k _, 

(57) 


VII.2 Measurement Update 

From the preceding discussion of the partial reset 
operation and the developments presented in Section VI, 
it is obvious that the measurement update is performed as 
follows. 

Measurement update 

Signal : 

e k (+) = e k (-) + K k (y k -y k ) (58.a) 







H 1 ^ 

t k _, h Time 

(a) The fully reset case 


H H ^ 

t k _j Time 

(b) The partially reset case 


Fig. 1 1 : Fully versus partially reset case 


9 

American Institute of Aeronautics and Astronautics 





X] c (+)= ^k(") + ^k( + ) 

Covariance: 

P t ( + ) = [I-K k H k ]P k (-)[I-K k HJ T 
+ K k R k Kj 

VII.3 Reset Update 

Although the change from X k (+) to X‘(+), shown 

in Fig. ll.b, can be of any nature, because we are 
dealing with the normalization operation, we chose the 
particular change 

x;(+) = X l (+)/||X l (+)| (60 -a) 

It is clear from Fig. I7.b that once X[(+) is defined, the 
value of e* (+) is also defined as 

(+) = X k (+) - X k (+) (60. b) 


(5 8.b) 
(59) 


Note that the condition of partially reset state vector 
does not come about by adding Aj. to X^(-) but 
rather by subtracting e*(+) from the fully reset state 

vector X k (+). As explained before, the partial reset 

operation does not influence the covariance matrix; 
therefore, the reset update is performed as follows. 


Reset update 

Signal: 


x;(+)=x k (+)/||x k (+)[ 

(61. a) 

e’ k (+) = x k (+)-£;(+) 

(61. b) 

Covariance : 


p k *(+)=Pk(+) 

(62) 

Both the fully and partially reset AEKF are 

summarized 


in Table I. 

VIII. THE REGULARITY OF THE AEKF FOR 
QUATERNION ESTIMATION 

After presenting some illustrative examples and 
having prepared the theoretical background we can now 
analyze the results of the examples. We start with 
quaternion estimation using the AEKF without 
normalization of the estimated quaternion. 

Quaternion estimation without normalization 

We presented two cases of quaternion estimation; 
namely, a static case with quaternion measurements and 
a dynamic case with vector measurements. The former 
constituted a strictly linear estimation problem that 
required the use of a standard KF. In the standard KF 
there is no connection between the quaternion estimate 
and the filter covariance matrix. Therefore, theoretically, 
the normality quality of the true quaternion had no 


Table I; AEKF with Full and Partial Reset 


System Model: 

Dynamics: 

Measurement: 

t k = A k X k-l + w k-l 
y k = h(X k )+ v k 

AEKF with Linear 

AEKF with Linear Dynamics 

Dynamics and Full Reset 

and Partial Reset 

Time Propagation: 

Time Propagation: 

X k ( _ ) ~ A k _,X k . (+) 

x k (-)=A k _,x;_,(+) 

Pk-l(-) = A k -,P k -i(+) 

^k(~) = A k-l^k-l( + ) 

A k _l +Q k -l 

Pk-l(-)«A k _,Pk_,<+) 
A k _l +Qk-1 

Measurement Update: 

Measurement Update: 

e k (+) = K k (y k -y k ) 

e k (+) = e k (-) 

X k (+) = X k (-) + e k (+) 

+K k (y k -y k ) 

Consequently: 


A 

( + ) = Xk (“) + £}< ( + ) 

X, (+) = X k (-) 

Consequently: 

+ K k (y k -y k ) 

X k (+) = X k (-) + e k (-) 

P k (+) = [I-K k HJP k (-)- 

+ K k (y k -y k ) 

[I-K k H k ] T 

Pk(+) = [I-K k H k ]P k (-) 

+ K k R k K k 

[l-K k H k ] T 
+ K k R k K k 

Reset Update: 

X(+) = X (+)/||x (+)|| 

*» = X, (+)-£(+) 
P k ‘(+) = P k W 


bearing on the covariance matrix. Indeed for this 
example we derived an analytic expression that exhibited 
no singularity. 

In the more complicated case with a rotating body 
and vector measurements, the dynamics equation is linear 
but the measurement equation is nonlinear. In that case 
we used the AEKF and still found no singularity. The fact 
that the true quaternion is normal does not enter into the 
computation of the covariance matrix. The only special 
feature of this case is the orthogonality of the transition 
matrix that propagates q, which preserves the norm of the 
propagated state vector be it normal or not. 

Quaternion estimation with normalization 

Forcing normalization on the a-posteriori estimate of 
the quaternion does not affect the covariance matrix. The 
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partially reset operation is performed as follows. First, 
the quaternion is updated as 


qk(+) = qk(-) + M + ) 


where e k (+) is the estimate of the difference 
q k _q k (_). Now, the forced normalization yields 


q k (+) as follows 


Ak(+)- 


qk(+) 

M+)|| 


(64) 


According to (61 b), the remaining error term is given as 

(65) 


A * 
e k 


: qk(+)-]r— TT 


This is a realization of the partially reset case of the 
AEKF depicted in Fig. (11 .b) and listed on the right 
column of Table I. We note that the only difference 
between the partially reset AEKF and the fully reset 
AEKF (which, as mentioned, is actually the ordinary 

EKF) is in the propagation of X k (+), which here is 
q*(+), and of e*(+) . (There are cases where in practice 
the filter performs well even when e k (+) is not 
propagated. It is assumed that the reason for it is that 
e’ (4-) , which is caused by the normalization operation, 

is quite small and/or the measurement update subdues 
any divergence tendency.) Since the partially reset 
operation has no effect on the covariance matrix 
computation, the normalization operation has no effect 
on the covariance of the AEKF. With or without 
normalization, we obtain the same nonsingular 
covariance matrix, P. 


IX. CONCLUSIONS 

The purpose of this paper is to explain that using an 
AEKF for estimating the quaternion-of-rotation does not 
result in a singular covariance matrix. We started this 
paper by presenting a conceptual example of estimating 
the vertices of a rotating square where, in spite of the 
geometric connection that exists between three of the 
vertices and the fourth one, there is no reason to assume 
that such a connection exists between estimates of the 
vertices. This constituted an analogy’ to the case of 
estimating the quaternion-of-rotation where there is no 
reason to assume that there is a functional relationship 
between the estimates of the quaternion elements 
although there is a connection between the elements of 
the true quaternion. 

Advancing from the qualitative example to a 
quantitative one, we presented a KF that estimated the 


position of the edges of a sliding rod the length of which 
was constant. In that example we checked the singularity 
of the covariance matrix both when the constant length 
between the estimated edges was not forced and when we 
did force it. In both cases the covariance matrix exhibited 
no singularity. Moreover, the matrix eigenvalues were 
identical in both cases. 

Following these general examples we moved to the 
case of quaternion estimation. We started with a particular 
case of a quaternion estimation problem of a rigid body 
with constant attitude and quaternion measurements. In 
this simple example where normalization was not forced 
we proved analytically that the covariance matrix is not 
singular. Finally we presented a case with a rotating rigid 
body and vector measurements. Again, the computed 
covariance matrix exhibited no singularity. 

In order to explain the results obtained in these 
examples we presented the ordinary EKF algorithm as a 
full reset estimation problem and, in parallel, we 
presented an EKF version as a partial reset estimation 
problem. It was shown that in either algorithm no 
inherent covariance matrix singularity is involved. 
Finally it was shown that the AEKF for estimating the 
quaternion with no forced normalization is an EKF with 
full reset, and when normalization is forced, the 
algorithm is an EKF with partial reset, and as mentioned, 
in* either case there is no inherent singularity in the 
covariance matrix. Indeed, in numerous runs of the 
AEKF under diverse conditions, with simulated and real 
data, with and without normalization, the covariance 
matrix was never singular. 
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